/*
IN1  - PA8-UH     -D7
IN2  - PA9-VH     -D8
IN3  - PA10-WH    -D2

EN1  - PC10
EN2  - PC11
EN3  - PC12

BKIN - PA6      -D5
*/

#include <Arduino.h>
// Open loop motor control example
#include <SimpleFOC.h>

// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number, phase resistance (optional) );
BLDCMotor motor = BLDCMotor(7);

// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 2, 9); //9 5 6

// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&motor.target, cmd); }



void setup() {
  // put your setup code here, to run once:
  pinMode(13, OUTPUT);

  /* en */
  pinMode(PC10, OUTPUT);
  digitalWrite(PC10, HIGH); 

  pinMode(PC11, OUTPUT);
  digitalWrite(PC11, HIGH); 

  pinMode(PC12, OUTPUT);
  digitalWrite(PC12, HIGH); 

  pinMode(PA6, OUTPUT);
  digitalWrite(PA6, HIGH); 

  Serial.begin(115200);
  Serial.println("start……");

    /* simple foc setup */
    // pwm frequency to be used [Hz]
    // esp32/stm32/teensy configurable
    //driver.pwm_frequency = 25000;
    // power supply voltage [V]
    driver.voltage_power_supply = 10;
    driver.init();

    // link the motor and the driver
    motor.linkDriver(&driver);

    // limiting motor movements
    // motor.phase_resistance = 3.52 // [Ohm]
    // motor.current_limit = 2;   // [Amps] - if phase resistance defined
    motor.voltage_limit = 3;   // [V] - if phase resistance not defined
    motor.velocity_limit = 500; // [rad/s] cca 50rpm

    // open loop control config
    motor.controller = MotionControlType::velocity_openloop;
    /*
    开环控制尽快操作，注意电机发热情况，发热严重要立即断电
     1、电机抖动转不起来把voltage_limit设置的大一点，
     2、电机发热严重的把voltage_limit设置的小一点，
     3、电机能转就表示一切正常，可以学习下一章了，开环不是电机控制的常态，不要纠结太久。20221203


    - 选择controller=Type_velocity_openloop;
    - 电源电压和极对数根据实际情况设置；电源电压增加voltage_limit也要相应的增大
    - voltage_limit在不了解电机性能的情况下从小逐渐增加，每次修改后注意工作电流和电机发热；
    - 如果电机功率较小而voltage_limit偏小，电机会转不动；
    - 如果电机功率较大而voltage_limit偏大，电机会发热严重。

    */

    // init motor hardware
    motor.init();

    // add target command T
    command.add('T', doTarget, "target velocity");


  /* Others */
    //_delay(1000);

    for(int i = 0; i < 10; i++){
        digitalWrite(13, HIGH); 
        delay(50);
        digitalWrite(13, LOW);  
        delay(50);
    }

    Serial.begin(115200);
    Serial.println("Motor ready!");
    Serial.println("Set target velocity [rad/s]");
}

void loop() {
    // put your main code here, to run repeatedly:
 
    // open loop velocity movement
    // using motor.voltage_limit and motor.velocity_limit
    motor.move(50);

    // user communication
    command.run();
}

